/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class RobotMain extends IterativeRobot {
    Joystick m_leftStick = new Joystick(1);
    Joystick m_rightStick = new Joystick(2);
    Victor m_testMotor = new Victor(1);             //TODO: Get chanel
    SendablePIDController m_PIDLoop;
    Encoder m_encoder = new Encoder(1, 2);          //TODO: Get chanels
    static final double kDistancePerPulse = 1.0;    //TODO: Get distance per pulse
    static final double kP = 1.0;                   //TODO: Tune P
    static final double kI = 1.0;                   //TODO: Tune I
    static final double kD = 1.0;                   //TODO: Tune D
    static final double kMaxSpeed = 100;            //TODO: Tune value.

    public void robotInit() {
        m_encoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        m_encoder.setDistancePerPulse(kDistancePerPulse);
        m_PIDLoop = new SendablePIDController(kP, kI, kD, m_encoder, m_testMotor);
        SmartDashboard.putData("PID Loop", m_PIDLoop);
        System.out.println("Build Complete!");
    }

    public void disabledInit() {
        m_PIDLoop.disable();
    }

    public void teleopInit() {
        m_PIDLoop.enable();
    }

    public void teleopPeriodic() {
        m_PIDLoop.setSetpoint(m_rightStick.getY() * kMaxSpeed);
    }
}